#include "stdafx.h"
#include "gp_Trsf.h"
#include "gp_Pnt2d.h"
#include "gp_GTrsf2d.h"
#include "gp_Trsf.h"
#include "gp_Pnt2d.h"
#include "gp_Ax2d.h"
#include "gp_Vec2d.h"
#include "gp_XY.h"
#include "gp_Mat2d.h"
#include "gp_Trsf2d.h"
#include "gp.h"


void gp_Trsf2d::SetMirror (const gp_Ax2d& A)
{
	shape = gp_Ax1Mirror;
	scale = - 1.0;
	const gp_Dir2d& V = A.Direction ();
	const gp_Pnt2d& P = A.Location ();
	double VX = V.X();
	double VY = V.Y();
	double X0 = P.X();
	double Y0 = P.Y();
	matrix.SetCol (1, gp_XY (1.0 - 2.0 * VX * VX, -2.0 * VX * VY));
	matrix.SetCol (2, gp_XY (-2.0 * VX * VY, 1.0 - 2.0 * VY * VY));

	loc.SetCoord  (-2.0 * ((VX * VX - 1.0) * X0 + (VX * VY * Y0)),
		-2.0 * ((VX * VY * X0) + (VY * VY - 1.0) * Y0));
}

void gp_Trsf2d::SetTransformation (const gp_Ax2d& FromA1,
	const gp_Ax2d& ToA2)
{
	shape = gp_CompoundTrsf;
	scale = 1.0;
	//matrix from XOY to A2 :
	const gp_XY& V1 = ToA2.Direction().XY();
	gp_XY V2 (-V1.Y(), V1.X());
	matrix.SetCol (1, V1);
	matrix.SetCol (2, V2);
	loc = ToA2.Location().XY();
	matrix.Transpose();
	loc.Multiply (matrix);
	loc.Reverse();
	//matrix FromA1 to XOY
	const gp_XY& V3 = FromA1.Direction().XY();
	gp_XY V4 (-V3.Y(), V3.X());
	gp_Mat2d MA1 (V3, V4);
	gp_XY MA1loc = FromA1.Location().XY();
	//matrix * MA1 => FromA1 ToA2
	MA1loc.Multiply (matrix);
	loc.Add (MA1loc);
	matrix.Multiply (MA1);
}

void gp_Trsf2d::SetTransformation (const gp_Ax2d& A)
{
	shape = gp_CompoundTrsf;
	scale = 1.0;
	const gp_XY& V1 = A.Direction().XY();
	gp_XY V2 (-V1.Y(), V1.X());
	matrix.SetCol (1, V1);
	matrix.SetCol (2, V2);
	loc = A.Location().XY();
	matrix.Transpose();
	loc.Multiply (matrix);
	loc.Reverse();
}

void gp_Trsf2d::SetTranslationPart (const gp_Vec2d& V)
{   
	loc = V.XY();
	double X = loc.X();
	if (X < 0) X = - X;
	double Y = loc.Y();
	if (Y < 0) Y = - Y;
	if (X <= gp::Resolution() && Y <= gp::Resolution()) {
		if (shape == gp_Identity  || shape == gp_PntMirror ||
			shape == gp_Scale     || shape == gp_Rotation  ||
			shape == gp_Ax1Mirror ) { }
		else if (shape == gp_Translation) { shape = gp_Identity; }
		else { shape = gp_CompoundTrsf; }
	}
	else {
		if (shape == gp_Translation || shape == gp_Scale ||
			shape == gp_PntMirror) { }
		else if (shape == gp_Identity) { shape = gp_Translation; }
		else { shape = gp_CompoundTrsf; }
	}
}

void gp_Trsf2d::SetScaleFactor (const double S)
{   
	if (S == 1.0) {
		double X = loc.X();
		if (X < 0) X = - X;
		double Y = loc.Y();
		if (Y < 0) Y = - Y;
		if (X <= gp::Resolution() && Y <= gp::Resolution()) {
			if (shape == gp_Identity || shape == gp_Rotation) { }
			else if (shape == gp_Scale)  { shape = gp_Identity; }
			else if (shape == gp_PntMirror) { shape = gp_Translation; }
			else { shape = gp_CompoundTrsf; }
		}
		else {
			if (shape == gp_Identity || shape == gp_Rotation ||
				shape == gp_Scale)  { }
			else if (shape == gp_PntMirror) { shape = gp_Translation; }
			else { shape = gp_CompoundTrsf; }
		}
	}
	else if (S == -1) {
		if (shape == gp_PntMirror || shape == gp_Ax1Mirror) { }
		else if (shape == gp_Identity || shape == gp_Scale) { 
			shape = gp_PntMirror;
		}
		else { shape = gp_CompoundTrsf; }
	}
	else {
		if (shape == gp_Scale) { }
		else if (shape == gp_Identity || shape == gp_Translation ||
			shape == gp_PntMirror) { shape = gp_Scale; }
		else { shape = gp_CompoundTrsf; }
	}
	scale = S;
}

gp_Mat2d gp_Trsf2d::VectorialPart () const
{ 
	if (scale == 1.0)  return matrix;
	gp_Mat2d M = matrix;
	if (shape == gp_Scale || shape == gp_PntMirror)
		M.SetDiagonal (matrix.Value(1,1) * scale, matrix.Value(2,2) * scale);
	else
		M.Multiply (scale);
	return M;
}

double gp_Trsf2d::RotationPart () const
{ 
	return ATan2 ( matrix.Value(2,1), matrix.Value(1,1) );
}

void gp_Trsf2d::Invert()
{ 
	//                                    -1
	//  X' = scale * R * X + T  =>  X = (R  / scale)  * ( X' - T)
	//
	// Pour les gp_Trsf2d puisque le scale est extrait de la matrice R
	// on a toujours determinant (R) = 1 et R-1 = R transposee.
	if (shape == gp_Identity) { }
	else if ( shape == gp_Translation || shape == gp_PntMirror) { 
		loc.Reverse(); 
	}
	else if ( shape == gp_Scale) {
		double As = scale;
		if (As < 0) As = - As;
		Standard_ConstructionError_Raise_if(As <= gp::Resolution(),""); 
		scale = 1.0 / scale;
		loc.Multiply (-scale);
	}
	else {
		double As = scale;
		if (As < 0) As = - As;
		Standard_ConstructionError_Raise_if(As <= gp::Resolution(),""); 
		scale = 1.0 / scale;
		matrix.Transpose();
		loc.Multiply (matrix);
		loc.Multiply (-scale);
	}
}

void gp_Trsf2d::Multiply(const gp_Trsf2d& T)
{
	if (T.shape == gp_Identity) { }
	else if (shape == gp_Identity) {
		shape = T.shape;
		scale = T.scale;
		loc = T.loc;
		matrix = T.matrix;
	} 
	else if (shape == gp_Rotation && T.shape == gp_Rotation) { 
		if (loc.X() != 0.0 || loc.Y() != 0.0) {
			loc.Add (T.loc.Multiplied (matrix));
		}
		matrix.Multiply(T.matrix);
	}
	else if (shape == gp_Translation && T.shape == gp_Translation) {
		loc.Add (T.loc);
	}
	else if (shape == gp_Scale && T.shape == gp_Scale) {
		loc.Add (T.loc.Multiplied(scale));
		scale = scale * T.scale;
	}
	else if (shape == gp_PntMirror && T.shape == gp_PntMirror) {
		scale = 1.0;
		shape = gp_Translation;
		loc.Add (T.loc.Reversed());
	}
	else if (shape == gp_Ax1Mirror && T.shape == gp_Ax1Mirror) {
		shape = gp_Rotation;
		gp_XY Tloc (T.loc);
		Tloc.Multiply (matrix);
		Tloc.Multiply (scale);
		scale = scale * T.scale;
		loc.Add (Tloc);
		matrix.Multiply (T.matrix);
	}
	else if ((shape == gp_CompoundTrsf || shape == gp_Rotation ||
		shape == gp_Ax1Mirror) && T.shape == gp_Translation) {
			gp_XY Tloc (T.loc);
			Tloc.Multiply (matrix);
			if (scale != 1.0) Tloc.Multiply (scale);
			loc.Add (Tloc);
	}
	else if ((shape == gp_Scale || shape == gp_PntMirror)
		&& T.shape == gp_Translation) {
			gp_XY Tloc (T.loc);
			Tloc.Multiply (scale);
			loc.Add (Tloc);
	}
	else if (shape == gp_Translation &&
		(T.shape == gp_CompoundTrsf ||
		T.shape == gp_Rotation || T.shape == gp_Ax1Mirror)) {
			shape = gp_CompoundTrsf;
			scale = T.scale;
			loc.Add (T.loc);
			matrix = T.matrix;
	}
	else if (shape == gp_Translation && 
		(T.shape == gp_Scale || T.shape == gp_PntMirror)) {
			shape = T.shape;
			loc.Add (T.loc);
			scale = T.scale;
	}
	else if ((shape == gp_PntMirror || shape == gp_Scale) &&
		(T.shape == gp_PntMirror || T.shape == gp_Scale)) {
			shape = gp_CompoundTrsf;
			gp_XY Tloc (T.loc);
			Tloc.Multiply (scale);
			loc.Add (Tloc);
			scale = scale * T.scale;
	}
	else if ((shape == gp_CompoundTrsf || shape == gp_Rotation ||
		shape == gp_Ax1Mirror)
		&& (T.shape == gp_Scale || T.shape == gp_PntMirror)) {
			shape = gp_CompoundTrsf;
			gp_XY Tloc (T.loc);
			Tloc.Multiply(matrix);
			if (scale == 1.0)  scale = T.scale;
			else {
				Tloc.Multiply (scale);
				scale = scale * T.scale;
			}
			loc.Add (Tloc);
	}
	else if ((T.shape == gp_CompoundTrsf || T.shape == gp_Rotation ||
		T.shape == gp_Ax1Mirror)
		&& (shape == gp_Scale || shape == gp_PntMirror)) {
			shape = gp_CompoundTrsf;
			gp_XY Tloc (T.loc);
			Tloc.Multiply (scale);
			scale = scale * T.scale;
			loc.Add (Tloc);
			matrix = T.matrix;
	}
	else {
		shape = gp_CompoundTrsf;
		gp_XY Tloc (T.loc);
		Tloc.Multiply (matrix);
		if (scale != 1.0) {
			Tloc.Multiply (scale);
			scale = scale * T.scale;
		}
		else { scale = T.scale; }
		loc.Add (Tloc);
		matrix.Multiply (T.matrix);
	}
}

void gp_Trsf2d::Power (const int N)
{
	if (shape == gp_Identity) { }
	else {
		if (N == 0)  {
			scale = 1.0;
			shape = gp_Identity;
			matrix.SetIdentity();
			loc = gp_XY (0.0, 0.0);
		}
		else if (N == 1)  { }
		else if (N == -1) Invert();
		else {
			if (N < 0) Invert();
			if (shape == gp_Translation) { 
				int Npower = N;
				if (Npower < 0) Npower = - Npower;
				Npower--;
				gp_XY Temploc = loc;
				for(;;) {
					if (IsOdd(Npower))  loc.Add (Temploc);
					if (Npower == 1) break;
					Temploc.Add (Temploc);
					Npower = Npower/2;
				}
			}
			else if (shape == gp_Scale) {
				int Npower = N;
				if (Npower < 0) Npower = - Npower;
				Npower--;
				gp_XY Temploc = loc;
				double Tempscale = scale;
				for(;;) {
					if (IsOdd(Npower)) {
						loc.Add (Temploc.Multiplied(scale));
						scale = scale * Tempscale;
					}
					if (Npower == 1) break;
					Temploc.Add (Temploc.Multiplied(Tempscale));
					Tempscale = Tempscale * Tempscale;
					Npower = Npower/2;
				}
			}
			else if (shape == gp_Rotation) {
				int Npower = N;
				if (Npower < 0) Npower = - Npower;
				Npower--;
				gp_Mat2d Tempmatrix (matrix);
				if (loc.X() == 0.0 && loc.Y() == 0.0) {
					for(;;) {
						if (IsOdd(Npower))  matrix.Multiply (Tempmatrix);
						if (Npower == 1) break;
						Tempmatrix.Multiply (Tempmatrix);
						Npower = Npower/2;
					}
				}
				else {
					gp_XY Temploc = loc;
					for(;;) {
						if (IsOdd(Npower)) {
							loc.Add (Temploc.Multiplied (matrix));
							matrix.Multiply (Tempmatrix);
						}
						if (Npower == 1) break;
						Temploc.Add (Temploc.Multiplied (Tempmatrix));
						Tempmatrix.Multiply (Tempmatrix);
						Npower = Npower/2;
					}
				}
			}
			else if (shape == gp_PntMirror || shape == gp_Ax1Mirror) {
				if (IsEven (N)) {
					shape = gp_Identity;
					scale = 1.0;
					matrix.SetIdentity ();
					loc.SetCoord (0.0, 0.0);
				}
			}
			else {
				shape = gp_CompoundTrsf;
				int Npower = N;
				if (Npower < 0) Npower = - Npower;
				Npower--;
				matrix.SetDiagonal (scale*matrix.Value(1,1),
					scale*matrix.Value(2,2));
				gp_XY Temploc = loc;
				double Tempscale = scale;
				gp_Mat2d Tempmatrix (matrix);
				for(;;) {
					if (IsOdd(Npower)) {
						loc.Add ((Temploc.Multiplied (matrix)).Multiplied (scale));
						scale = scale * Tempscale;
						matrix.Multiply (Tempmatrix);
					}
					if (Npower == 1) break;
					Tempscale = Tempscale * Tempscale;
					Temploc.Add ( (Temploc.Multiplied (Tempmatrix)).Multiplied 
						(Tempscale)
						);
					Tempmatrix.Multiply (Tempmatrix);
					Npower = Npower/2;
				}
			}
		}
	}
}

void gp_Trsf2d::PreMultiply (const gp_Trsf2d& T)
{
	if (T.shape == gp_Identity) { }
	else if (shape == gp_Identity) {
		shape = T.shape;
		scale = T.scale;
		loc = T.loc;
		matrix = T.matrix;
	} 
	else if (shape == gp_Rotation && T.shape == gp_Rotation) { 
		loc.Multiply (T.matrix);
		loc.Add (T.loc);
		matrix.PreMultiply(T.matrix);
	}
	else if (shape == gp_Translation && T.shape == gp_Translation) {
		loc.Add (T.loc);
	}
	else if (shape == gp_Scale && T.shape == gp_Scale) {
		loc.Multiply (T.scale);
		loc.Add (T.loc);
		scale = scale * T.scale;
	}
	else if (shape == gp_PntMirror && T.shape == gp_PntMirror) {
		scale = 1.0;
		shape = gp_Translation;
		loc.Reverse();
		loc.Add (T.loc);
	}
	else if (shape == gp_Ax1Mirror && T.shape == gp_Ax1Mirror) {
		shape = gp_Rotation;
		loc.Multiply (T.matrix);
		loc.Multiply(T.scale);
		scale = scale * T.scale;
		loc.Add (T.loc);
		matrix.PreMultiply(T.matrix);
	}
	else if ((shape == gp_CompoundTrsf || shape == gp_Rotation ||
		shape == gp_Ax1Mirror) && T.shape == gp_Translation) {
			loc.Add (T.loc);
	}
	else if ((shape == gp_Scale || shape == gp_PntMirror)
		&& T.shape == gp_Translation) {
			loc.Add (T.loc);
	}
	else if (shape == gp_Translation &&
		(T.shape == gp_CompoundTrsf ||
		T.shape == gp_Rotation || T.shape == gp_Ax1Mirror)) {
			shape = gp_CompoundTrsf;
			matrix = T.matrix;
			if (T.scale == 1.0)  loc.Multiply (T.matrix);
			else {
				scale = T.scale;
				loc.Multiply (matrix);
				loc.Multiply (scale);
			}
			loc.Add (T.loc);
	}
	else if ((T.shape == gp_Scale || T.shape == gp_PntMirror)
		&& shape == gp_Translation) {
			loc.Multiply (T.scale);
			loc.Add (T.loc);
			scale = T.scale;
			shape = T.shape;
	}
	else if ((shape == gp_PntMirror || shape == gp_Scale) &&
		(T.shape == gp_PntMirror || T.shape == gp_Scale)) {
			shape = gp_CompoundTrsf;
			loc.Multiply (T.scale);
			loc.Add (T.loc);
			scale = scale * T.scale;
	}
	else if ((shape == gp_CompoundTrsf || shape == gp_Rotation ||
		shape == gp_Ax1Mirror)
		&& (T.shape == gp_Scale || T.shape == gp_PntMirror)) {
			shape = gp_CompoundTrsf;
			loc.Multiply (T.scale);
			loc.Add (T.loc);
			scale = scale * T.scale;
	} 
	else if ((T.shape == gp_CompoundTrsf || T.shape == gp_Rotation ||
		T.shape == gp_Ax1Mirror) 
		&& (shape == gp_Scale || shape == gp_PntMirror)) {
			shape = gp_CompoundTrsf;
			matrix = T.matrix;
			if (T.scale == 1.0)  loc.Multiply (T.matrix);
			else {
				loc.Multiply (matrix);
				loc.Multiply (T.scale);
				scale = T.scale * scale;
			}
			loc.Add (T.loc);
	} 
	else {
		shape = gp_CompoundTrsf;
		loc.Multiply (T.matrix);
		if (T.scale != 1.0) {
			loc.Multiply(T.scale);   scale = scale * T.scale;
		}
		loc.Add (T.loc);
		matrix.PreMultiply(T.matrix);
	}
}

 gp_Trsf2d::gp_Trsf2d () {
  shape = gp_Identity;
  scale = 1.0;
  matrix.SetIdentity ();
  loc.SetCoord (0.0, 0.0);
}

 gp_Trsf2d::gp_Trsf2d (const gp_Trsf& T) :
scale(T.ScaleFactor()),
shape(T.Form()),
loc(T.TranslationPart().X(),T.TranslationPart().Y())
{
  const gp_Mat& M = T.HVectorialPart();
  matrix(1,1) = M(1,1);
  matrix(1,2) = M(1,2);
  matrix(2,1) = M(2,1);
  matrix(2,2) = M(2,2);
}

 void gp_Trsf2d::SetMirror(const gp_Pnt2d& P)
{   
  shape = gp_PntMirror;
  scale = -1.0;
  matrix.SetIdentity ();
  loc = P.XY();
  loc.Multiply (2.0);
}

 void gp_Trsf2d::SetRotation (const gp_Pnt2d& P,
				    const double Ang)
{
  shape = gp_Rotation;
  scale = 1.0;
  loc = P.XY ();
  loc.Reverse ();
  matrix.SetRotation (Ang);
  loc.Multiply (matrix);
  loc.Add (P.XY());
}

 void gp_Trsf2d::SetScale (const gp_Pnt2d& P,
				 const double S)
{
  shape = gp_Scale;
  scale = S;
  matrix.SetIdentity ();
  loc = P.XY ();
  loc.Multiply (1.0 - S);
}

 void gp_Trsf2d::SetTranslation(const gp_Vec2d& V)
{
  shape = gp_Translation;
  scale = 1.0;
  matrix.SetIdentity ();
  loc = V.XY ();
}

 void gp_Trsf2d::SetTranslation (const gp_Pnt2d& P1,
				       const gp_Pnt2d& P2)
{
  shape = gp_Translation;
  scale = 1.0;
  matrix.SetIdentity ();
  loc = (P2.XY()).Subtracted (P1.XY());
}

 bool gp_Trsf2d::IsNegative() const
{ return (matrix.Determinant() < 0.0); }

 const gp_XY& gp_Trsf2d::TranslationPart () const
{ return loc; }

 const gp_Mat2d& gp_Trsf2d::HVectorialPart () const
{ return matrix; }

 double gp_Trsf2d::Value (const int Row,
				       const int Col) const
{
  Standard_OutOfRange_Raise_if
    (Row < 1 || Row > 2 || Col < 1 || Col > 3, " ");
  if (Col < 3) return scale * matrix.Value (Row, Col);
  else         return loc.Coord (Row);
}

 gp_TrsfForm gp_Trsf2d::Form() const
{ return shape; }

 double gp_Trsf2d::ScaleFactor() const
{ return scale; }

 gp_Trsf2d gp_Trsf2d::Inverted() const
{ 
  gp_Trsf2d T = *this;
  T.Invert();
  return T;
}

 gp_Trsf2d gp_Trsf2d::Multiplied (const gp_Trsf2d& T) const {
  gp_Trsf2d Tresult(*this);
  Tresult.Multiply(T);
  return Tresult;
}

 gp_Trsf2d gp_Trsf2d::Powered (const int N)
{
  gp_Trsf2d T = *this;
  T.Power (N);
  return T;
}

 void gp_Trsf2d::Transforms (double& X,
				   double& Y) const
{
  gp_XY Doublet (X, Y);
  Doublet.Multiply (matrix);
  if (scale != 1.0) Doublet.Multiply (scale);
  Doublet.Add(loc);
  Doublet.Coord (X, Y);
}

 void gp_Trsf2d::Transforms (gp_XY& Coord) const
{
  Coord.Multiply (matrix);
  if (scale != 1.0) Coord.Multiply (scale);
  Coord.Add(loc);
}

